% Pose estimation frame test 
clc; clear;

A = [0 0 ]';
B = [177 0]';
C = [177 177 ]';
D = [0 177]';

pose = [A B C D];
% FRAME 01:
% import cam para, M matrix

ptSample(1,1:4) = [211 209 391 395];
ptSample(2,1:4) = [237 52 46 233];

H = HomoMtrxCalc(pose, ptSample)

[R t] = Homo2Extr(H)

input('Press Enter');

% FRAME 02:
% import cam para, M matrix

ptSample(1,1:4) = [264 265 452 458];
ptSample(2,1:4) = [238 45 10 232];

H = HomoMtrxCalc(pose, ptSample)

[R t] = Homo2Extr(H) 

input('Press Enter');

% FRAME 03:
% import cam para, M matrix

ptSample(1,1:4) = [163 172 423 421];
ptSample(2,1:4) = [397 81 14 457];

H = HomoMtrxCalc(pose, ptSample)

[R t] = Homo2Extr(H) 

input('Press Enter');


A = [0  0]';
B = [105 0]';
C = [105 105]';
D = [0 105]';

pose = [A B C D];
% FRAME 04a

ptSample(1,1:4) = [347 343 209 207];
ptSample(2,1:4) = [215 462 412 215];

H = HomoMtrxCalc(pose, ptSample)

[R t] = Homo2Extr(H) 

input('Press Enter');
% FRAME 04b

ptSample(1,1:4) = [488 489 347 343];
ptSample(2,1:4) = [418 215 217 462];

H = HomoMtrxCalc(pose, ptSample)

[R t] = Homo2Extr(H) 





